#include "ros/ros.h"
#include "epuckDriver/SetOdometry.h"
#include "epuckDriver/ResetOdometry.h"
#include "epuckDriver/GetGeometry.h"
#include "epuckDriver/GetIRPose2D.h"


#include <unistd.h>

int main(int argc,char **argv){

    ros::init(argc,argv,"epuckRequestResponseTest");
    ros::NodeHandle n;
    ros::ServiceClient cli_set;
    ros::ServiceClient cli_res;
    ros::ServiceClient cli_getG;
    ros::ServiceClient cli_getIR;

    cli_set = n.serviceClient<epuckDriver::SetOdometry>("epuckSetOdom1");
    cli_res =
n.serviceClient<epuckDriver::ResetOdometry>("epuckResetOdom1");
    cli_getG =
n.serviceClient<epuckDriver::GetGeometry>("epuckGetGeom1");
    cli_getIR =
n.serviceClient<epuckDriver::GetIRPose2D>("epuckGetIRPose1");

    ros::Rate loop(1);
    
    while(ros::ok){
    ROS_INFO("GET GEOMETRY");
    epuckDriver::GetGeometry geom; 
    if(cli_getG.call(geom)){
        ROS_INFO("geom: OK");
        ROS_INFO("width: %f, height: %f",geom.response.width,geom.response.height);
    }
    
    ROS_INFO("GET IRPOSE2D");
    epuckDriver::GetIRPose2D ir;
    if( cli_getIR.call(ir)){
        ROS_INFO("ir: OK");
        ROS_INFO("count: %d",ir.response.count);       
        for(int i=0;i<ir.response.count; i++)
            ROS_INFO("position[x,y,theta]: %f %f %f",
                    ir.response.pose2d[i].x,
                    ir.response.pose2d[i].y,
                    ir.response.pose2d[i].theta);
    }
    
    ROS_INFO("RESET ODOMETRY");
    epuckDriver::ResetOdometry reset;
    if(cli_res.call(reset)){
        ROS_INFO("reset: ok");
        ROS_INFO("verify epuckOdometryPublisher");
    }

    ROS_INFO("SET ODOMETRY");
    epuckDriver::SetOdometry set;
    set.request.pose2d.x = 0.05;
    set.request.pose2d.y = 0.05;
    set.request.pose2d.theta = 0.05;
    if(cli_set.call(set)){
        ROS_INFO("set: ok");
        ROS_INFO("verify epuckOdometryPublisher");
    }
        loop.sleep();
    }
    return 0;
}

